#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "MPU6050.h"
#include "KALMAN_FILTER.h"
#include "Serial.h"


KalmanFilter kf;  // 声明滤波器实例

int main(void) {
    MPU6050_Init();
	OLED_Init();
	Serial_Init();
/**
  * @brief  卡尔曼滤波器初始化
  * @param  kf         滤波器实例指针
  * @param  Q_angle    过程噪声-角度（推荐值 0.001~0.003）
  * @param  Q_bias     过程噪声-零偏（推荐值 0.001~0.003）
  * @param  R_measure  测量噪声（推荐值 0.03~0.1）
  */
    Kalman_Init(&kf, 0.01f, 0.002f, 0.005f);  // 初始化滤波器参数
    OLED_ShowString(0,0,"MPU6050",OLED_8X16);
    while(1) {
        int16_t AccX, AccY, AccZ, GyroX, GyroY, GyroZ;
        MPU6050_GetData(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
        
        // 计算加速度计俯仰角（假设X轴朝前，Z轴垂直）
        float accel_angle = Get_PitchAngle(AccX, AccZ);
        
        // 陀螺仪Y轴角速度（根据实际安装方向调整）
        float gyro_rate = GyroY / 16.4f;  // 转换为°/s（±2000dps量程）
        
        // 更新卡尔曼滤波器（假设采样周期dt=0.01s，即100Hz）
        Kalman_Update(&kf, gyro_rate, accel_angle, 0.01f);
        
        // 获取滤波后的角度
        float filtered_angle = Kalman_GetAngle(&kf);
		OLED_Printf(0, 16, OLED_8X16, "Angle:%.3f", accel_angle);
        OLED_Printf(0, 32, OLED_8X16, "Gyr:%.3f", gyro_rate);
        OLED_Printf(0, 48, OLED_8X16, "upAngle:%.3f", filtered_angle);
		OLED_Update();
		
        Serial_Printf("%04.3f,%04.3f\r\n",accel_angle*100,filtered_angle*100);
        // 后续控制逻辑...
    }
}
